// Kevin Kemper

#include <fca_controllers/controller.h>
#include <rtai.h>

// Torque constant (Nm/Arms):
#define TC		-0.18

// Gear ratio:
#define GR		17.25

// Torque -> Amps
#define T2A		((1.0/TC)*(1.0/GR))



/*	INPUT members:

	float			motor_angle;
	float			leg_angle;

	float			motor_velocity;
	float			leg_velocity;

	unsigned char	motor_current;

	unsigned char	command;
*/



extern void initialize_openloop_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	OPENLOOP_SINE_CONTROLLER_STATE(state)->time = 0.0;
}


extern void update_openloop_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, ControllerData *data) {


	float t;
	
	float i		= OPENLOOP_SINE_CONTROLLER_DATA(data)->open_I;
	float b		= OPENLOOP_SINE_CONTROLLER_DATA(data)->open_B;
	float k		= OPENLOOP_SINE_CONTROLLER_DATA(data)->open_k;
	float w		= 2*PI*OPENLOOP_SINE_CONTROLLER_DATA(data)->open_f;
	float a		= OPENLOOP_SINE_CONTROLLER_DATA(data)->open_a;


	float c1,c2;	
	float u;
	

	OPENLOOP_SINE_CONTROLLER_STATE(state)->time += 0.001;
	output->time = OPENLOOP_SINE_CONTROLLER_STATE(state)->time;
	
		
	t	= OPENLOOP_SINE_CONTROLLER_STATE(state)->time;


	c1 = (i*w*w*w*b)/(w*w*b*b+k*k);
	c2 = (w*w*b*b-i*w*w*k+k*k)/(w*w*b*b+k*k);

	u = a*c1*cos(w*t) + a*c2*sin(w*t);
	
//	rt_printk( "amp %d   yd1 %d   x1 %d\n", (int)(T2A*u*10), (int)(yd1*1000),(int)(x1*1000) );

	if (u > OPENLOOP_SINE_CONTROLLER_DATA(data)->open_lim) {
		output->motor_torque =  T2A*OPENLOOP_SINE_CONTROLLER_DATA(data)->open_lim;
	}
	else if (u < -OPENLOOP_SINE_CONTROLLER_DATA(data)->open_lim) {
		output->motor_torque = -T2A*OPENLOOP_SINE_CONTROLLER_DATA(data)->open_lim;
	}
	else {
		output->motor_torque = T2A*u;
	}
	

}


extern void takedown_openloop_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	OPENLOOP_SINE_CONTROLLER_STATE(state)->time = 0.0;
}
